perm filename CAMERA.SAI[SYS,HE]4 blob
sn#032851 filedate 1973-04-02 generic text, type T, neo UTF8
00100 COMMENT ⊗ VALID 00010 PAGES
00200 RECORD PAGE DESCRIPTION
00300 00001 00001
00400 00002 00002 BEGIN "GILCAM"
00500 00005 00003 SIMPLE PROCEDURE DATXFR ⊃ READS DATA IN GILL'S FORMAT
00600 00008 00004 PROCEDURE PANTIL_CAM(INTEGER CLENSREAL PPOT,TPOT,FPOT
00700 00012 00005 SIMPLE MESSAGE PROCEDURE CAM_UPDATE
00800 00018 00006 SIMPLE MESSAGE PROCEDURE CHNG_LENS(INTEGER RLENS)
00900 00021 00007 SIMPLE MESSAGE PROCEDURE CAM_CENTER(INTEGER RLENS REAL XTC,YTC,ZTC)
01000 00024 00008
01100 00027 00009 SIMPLE PROCEDURE TESTIT
01200 00030 00010
01300 00031 ENDMK
01400 ⊗;
00100 BEGIN "GILCAM"
00200 REQUIRE "PREAMB.SAI[SYS,HE]" SOURCE_FILE;
00300 REQUIRE "HELIB[1,3]" LIBRARY;
00400 REQUIRE -1 NEW_ITEMS;
00500
00600 DEFINE ⊃="COMMENT";
00700
00800 ⊃ THIS IS THE CAMERA MINDER . IT HOUSES THE FOLLOWING MESSAGE PROCEDURES:
00900 CAM_INIT-INITIALIZES THE CAMERA MODEL FROM ONE OF IRWIN'S RELAXED MODELS.
01000 CAM_UPDATEATE-READS THE POTS AND CREATES A NEW CAMERA TRANSFORM MATRIX.
01100 CHNG_LENS(LENS), MOVE_CAM(PAN,TILT), CHNG_FOCUS(RANGE)-DO THE OBVIOUS.
01200 CAM_CENTER(LENS,X,Y,Z)-CENTER THE CAMERA ON TABLE COORDS X,Y,Z.
01300 CAM_PRED(LENS,X,Y,Z)-COMPUTE THE MODEL FOR THE CAMERA ON TABLE COORDS X,Y,Z,
01400 AND STORE IT INTO CAMERA_PREDI.
01500
01600 THE CAMERA TRANSFROM MATRIX IS A 10X3 MATRIX:
01700 1:3 X 1:3 THE COLINEATION MATRIX.
01800 4 X 1:3 THE LENS CENTER.
01900 5 X 1:3 THE CENTER OF THE FIELD OF VIEW
02000 6:8 X 1:3 THE INVERSE COLINEATION MATRIX.
02100 9 X 1:3 CAMPAN, CAMTIL, CAMRANG
02200 10 X 1:2 CAMERA NO., CAMLENS
02300
02400 A NEW ITEM IS GENERATED, PUT IN CURCAM, AND THIS MATRIX BECOMES
02500 ITS DATUM.
02600 ERRORS: 1-SERVO PROBLEMS(SERVO).
02700 2-LENS OUT OF BOUNDS(CHNG_LENS,CAM_CENTER).
02800 3-PAN OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
02900 4-TILT OUT OF BOUNDS(MOVE_CAM,CAM_CENTER).
03000 5-FOCUS OUT OF BOUNDS(CHNG_FOCUS,CAM_CENTER).
03100 6-LENS CHANGER GOOFED(CHNG_LENS).
03200 7-LOOKUP FAILED(DATXFR).
03300 8-NOT ENOUGH READINGS(CAM_UPDATE).
03400 9-POTS TOO NOISY(CAM_UPDATE).
03500 10-AD NOT AVAILABLE(CAM_UPDATE).
03600 ;
03700
03800 REAL ARRAY MCOL,MICOL [1:3,1:3]; ⊃ WHERE I KEEP CURRENT CAMERA XFORM;
03900 REAL ARRAY LCEN[1:3]; ⊃ AND LENS CENTER ;
04000
04100 REAL ARRAY PPOT0,PPOTD,TPOT0,TPOTD,FPOT0,FPOTD,MART,SWING,
04200 FOC,FOCLEN0,FOCLENG[1:4],DP,P0[1:4,1:3],PP[1:4,1:2];
04300 INTEGER MESS,BLOCK,DUMMY;
04400
04500 DEFINE XDATA="3",STX="1 STEP 1 UNTIL",CRLF="&'15&'12",
04600 YES="INCHWL=""Y""";
00100 SIMPLE PROCEDURE DATXFR; ⊃ READS DATA IN GILL'S FORMAT;
00200 BEGIN BOOLEAN FLAG;
00300 INTEGER DUMMY;
00400 DEFINE DATASET="""DATA[SYS,HE]""";
00500 DEFINE XFR(X)="ARRYIN(XDATA,X,1)";
00600 OPEN(XDATA,"DSK",12,3,0,0,0,0);
00700 LOOKUP(XDATA,DATASET,FLAG);
00800 IF FLAG THEN BEGIN OUTSTR("DATXFR-FAILED: LOOKUP FAILED FOR "&
00900 DATASET CRLF);
01000 CAMFLG←7; RETURN; END;
01100 IF TYP_CAM THEN OUTSTR("DATXFR: RETRIEVING "&DATASET&CVS(BLOCK)CRLF);
01200 USETI(XDATA,1); DUMMY←WORDIN(XDATA);
01300 USETI(XDATA,BLOCK);
01400 XFR(PPOT0[BLOCK]); XFR(PPOTD[BLOCK]); XFR(TPOT0[BLOCK]); XFR(TPOTD[BLOCK]);
01500 XFR(FPOT0[BLOCK]); XFR(FPOTD[BLOCK]);
01600 XFR(MART[BLOCK]); XFR(SWING[BLOCK]);
01700 XFR("PP[BLOCK,1]");XFR("PP[BLOCK,2]");XFR("P0[BLOCK,1]");XFR("P0[BLOCK,2]");
01800 XFR("P0[BLOCK,3]");XFR("DP[BLOCK,1]");XFR("DP[BLOCK,2]");XFR("DP[BLOCK,3]");
01900 XFR(FOC[BLOCK]); XFR(FOCLEN0[BLOCK]); XFR(FOCLENG[BLOCK]);
02000 RELEASE(XDATA);
02100 END "DATXFR";
02200
02300 SIMPLE PROCEDURE SERVO;
02400 BEGIN
02500 EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
02600 EXTERNAL REAL L1,L2,L3,P1,P2,P3,E1,E2,E3,REF;
02700 EXTERNAL INTEGER STATUS,TSERVO;
02800 EXTERNAL PROCEDURE SPWOFF;
02900
03000 SPWON(1,TSERVO);
03100 WHILE ¬(STATUS LAND 1) DO;
03200 IF ((STATUS LAND '77777)≥'100) THEN
03300 BEGIN
03400 OUTSTR("SERVO-FAILED: STATUS="&CVOS(STATUS)CRLF);
03500 CAMFLG←1;
03600 END;
03700 SPWOFF;
03800 CALL(1,"SLEEP");
03900 IF DEB_CAM THEN
04000 BEGIN
04100 OUTSTR("SERVO: E1="&CVS(E1)&" E2="&CVS(E2)&" E3="&CVS(E3)CRLF);
04200 OUTSTR(" L1="&CVS(L1*REF)&" L2="&CVS(L2*REF)&" L3="&CVS(L3*REF)CRLF);
04300 OUTSTR(" P1="&CVS(P1*REF)&" P2="&CVS(P2*REF)&" P3="&CVS(P3*REF)CRLF);
04400 OUTSTR(" REF="&CVS(REF)CRLF);
04500 END;
04600 END "SERVO";
04700
04800 SIMPLE REAL PROCEDURE LNS(REAL X; INTEGER RLENS);
04900 RETURN (X*FOC[RLENS]/(X-FOC[RLENS]));
00100 PROCEDURE PANTIL_CAM(INTEGER CLENS;REAL PPOT,TPOT,FPOT;
00200 REAL ARRAY COL,ICOL,CENTER);
00300 ⊃ Compute the transformation matrices for the camera model
00400 given by LENS and the last pots readings;
00500 BEGIN INTEGER I,J;
00600 REAL ACC,FMX,FMY,PAN,TILT;
00700 REAL ARRAY RP,RT,RPT,RS,R[1:3,1:3],C[1:3];
00800 FORTRAN REAL PROCEDURE SIN;
00900 FORTRAN REAL PROCEDURE COS;
01000 REQUIRE "SOBMAT[SYS,HE]" LOAD_MODULE;
01100 EXTERNAL PROCEDURE INVRT(REAL ARRAY A,AI);
01200
01300 PAN←PPOTD[CLENS]*PPOT+PPOT0[CLENS];
01400 TILT←TPOTD[CLENS]*TPOT+TPOT0[CLENS];
01500 FMY←FPOTD[CLENS]*FPOT+FPOT0[CLENS]; FMX←FMY*MART[CLENS];
01600
01700 RP[2,3]←-1; RPT[1,1]←RP[1,1]←RP[3,2]←-SIN(PAN);
01800 RP[3,1]←-(RPT[1,2]←RP[1,2]←COS(PAN));
01900 RT[1,1]←1; RPT[2,3]←-(RT[2,2]←RT[3,3]←COS(TILT));
02000 R[3,3]←RPT[3,3]←RT[2,3]←-(RT[3,2]←SIN(TILT));
02100 RPT[2,1]←RT[2,3]*RP[3,1]; RPT[2,2]←RT[2,3]*RP[3,2];
02200 R[3,1]←RPT[3,1]←RT[3,3]*RP[3,1]; R[3,2]←RPT[3,2]←RT[3,3]*RP[3,2];
02300 RS[3,3]←1; RS[1,1]←RS[2,2]←COS(SWING[CLENS]);
02400 RS[2,1]←-(RS[1,2]←SIN(SWING[CLENS]));
02500 R[1,1]←RS[1,1]*RPT[1,1]+RS[1,2]*RPT[2,1];
02600 R[1,2]←RS[1,1]*RPT[1,2]+RS[1,2]*RPT[2,2]; R[1,3]←RS[1,2]*RPT[2,3];
02700 R[2,1]←RS[2,1]*RPT[1,1]+RS[2,2]*RPT[2,1];
02800 R[2,2]←RS[2,1]*RPT[1,2]+RS[2,2]*RPT[2,2]; R[2,3]←RS[2,2]*RPT[2,3];
02900
03000 C[1]←P0[CLENS,1]+R[1,1]*DP[CLENS,1]+R[2,1]*DP[CLENS,2]+R[3,1]*DP[CLENS,3];
03100 C[2]←P0[CLENS,2]+R[1,2]*DP[CLENS,1]+R[2,2]*DP[CLENS,2]+R[3,2]*DP[CLENS,3];
03200 C[3]←P0[CLENS,3]+R[1,3]*DP[CLENS,1]+R[2,3]*DP[CLENS,2]+R[3,3]*DP[CLENS,3];
03300
03400 ⊃ -FMX/333 and PP[2]-PP[1]/333 are corrections for
03500 the non-orthogonality of the TV scan axes;
03600 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
03700 COL[I,1]←R[I,1]; COL[I,2]←R[I,2]; ACC←0;
03800 FOR J←1 STEP 1 UNTIL 3 DO ACC←ACC-R[I,J]*C[J];
03900 COL[I,3]←ACC END;
04000 FOR J←1 STEP 1 UNTIL 3 DO BEGIN
04100 COL[2,J]←-FMX/333*COL[1,J]+FMY*COL[2,J]
04200 +(PP[CLENS,2]-PP[CLENS,1]/333)*COL[3,J];
04300 COL[1,J]←FMX*COL[1,J]+PP[CLENS,1]*COL[3,J]; END;
04400 INVRT(COL,ICOL); ARRTRAN(CENTER,C);
04500 END "PANTIL_CAM";
00100 SIMPLE MESSAGE PROCEDURE CAM_UPDATE;
00200 BEGIN REAL SFOC,STIL,SPAN,IND,FMAX,FMIN,TMAX,TMIN,PMAX,PMIN,
00300 DIFFOC,DIFTIL,DIFPAN,SIND;
00400 EXTERNAL REAL L1,L2,L3,P1,P2,P3,REF; EXTERNAL INTEGER STATUS,TSERVO;
00500 INTEGER UPDFLG;
00600 FORTRAN REAL PROCEDURE SQRT(REAL X);
00700 EXTERNAL PROCEDURE SPWON(INTEGER TIC;REFERENCE INTEGER ADDR);
00800 EXTERNAL PROCEDURE SPWOFF;
00900 LABEL ETA;
01000
01100 ETA: SFOC←STIL←SPAN←0; FMAX←TMAX←PMAX←-10000; FMIN←TMIN←PMIN←10000;
01200 STATUS←1; SPWON(1,TSERVO);
01300 FOR IND←0 STEP 1 UNTIL 39 DO BEGIN
01400 STATUS←4; WHILE ¬(STATUS LAND 1) DO;
01500 IF (STATUS≥'100)∧(STATUS<'100000) THEN DONE;
01600 SFOC←SFOC+P1; STIL←STIL+P2; SPAN←SPAN+P3;
01700 IF P1>FMAX THEN FMAX←P1; IF P1<FMIN THEN FMIN←P1;
01800 IF P2>TMAX THEN TMAX←P2; IF P2<TMIN THEN TMIN←P2;
01900 IF P3>PMAX THEN PMAX←P3; IF P3<PMIN THEN PMIN←P3;
02000 END;
02100 SPWOFF;
02200
02300 IF IND>0
02400 THEN BEGIN FOCPOT←SFOC*REF/IND; TILPOT←STIL*REF/IND; PANPOT←SPAN*REF/IND;
02500 L1 ← SFOC/IND; L2 ← STIL/IND; L3 ← SPAN/IND;
02600 IF DEB_CAM THEN OUTSTR("CAM_UPDATE: "&
02700 "FP="&CVG(FOCPOT)&" TP="&CVG(TILPOT)&" PP="&CVG(PANPOT)CRLF);
02800
02900 IF IND<30
03000 THEN BEGIN OUTSTR("CAM_UPDATE: NOT ENOUGH READINGS ("&
03100 CVS(IND)&")"CRLF); UPDFLG←8; END
03200 ELSE BEGIN DIFFOC←(FMAX-FMIN)*REF; DIFTIL←(TMAX-TMIN)*REF; DIFPAN←(PMAX-PMIN)*REF;
03300 IF DEB_CAM THEN OUTSTR("CAM_UPDATE: "&
03400 "DF="&CVS(DIFFOC)&" DT="&CVS(DIFTIL)&" DP="&CVS(DIFPAN)CRLF);
03500 SIND←4*SQRT(IND);
03600 IF (DIFPAN/SIND>.5)∨(DIFTIL/SIND>.25)∨(DIFFOC/SIND>1)
03700 THEN BEGIN
03800 OUTSTR("CAM_UPDATE: POTS TOO NOISY ("&CVS(DIFFOC)&" "&
03900 CVS(DIFTIL)&" "&CVS(DIFPAN)&")"CRLF);
04000 UPDFLG←9; END; END; END
04100 ELSE BEGIN OUTSTR("CAM_UPDATE: AD NOT AVAILABLE"CRLF); UPDFLG←10; END;
04200 IF UPDFLG≠0 THEN BEGIN
04300 OUTSTR("...TYPE Y TO TRY AGAIN:"CRLF);
04400 IF YES
04500 THEN BEGIN UPDFLG←0; GOTO ETA; END
04600 ELSE BEGIN CAMFLG←UPDFLG;
04700 IF CAMFLG=10 THEN BEGIN
04800 OUTSTR("CAM_UPDATE-FAILED: CAMERA_MODEL NOT UPDATED"CRLF); RETURN;
04900 END; END; END;
05000
05100 PANTIL_CAM (CAMLENS,PANPOT,TILPOT,FOCPOT,MCOL,MICOL,LCEN);
05200
05300 ⊃ Now to update the global model;
05400
05500 ARRBLT (CAMERA_MODEL[1,1],MCOL[1,1],9);
05600 ARRBLT (CAMERA_MODEL[6,1],MICOL[1,1],9);
05700 ARRBLT (CAMERA_MODEL[4,1],LCEN[1],3);
05800 CAMERA_MODEL[5,1] ← PP[CAMLENS,1];
05900 CAMERA_MODEL[5,2] ← PP[CAMLENS,2];
06000 CAMERA_MODEL[5,3] ← 1.0;
06100 CAMERA_MODEL[9,1]←CAMPAN←PPOTD[CAMLENS]*PANPOT+PPOT0[CAMLENS];
06200 CAMERA_MODEL[9,2]←CAMTIL←TPOTD[CAMLENS]*TILPOT+TPOT0[CAMLENS];
06300 CAMERA_MODEL[9,3]←CAMRANG←LNS(FOCLEN0[CAMLENS]+
06400 FOCLENG[CAMLENS]*FOCPOT,CAMLENS);
06500 CAMERA_MODEL[10,2]←CAMLENS;
06600 CURCAM ← GLOBAL NEW (CAMERA_MODEL);
06700
06800 IF DEB_CAM THEN OUTSTR("CAM_UPDATE: "&
06900 "CAMPAN="&CVG(CAMPAN)&" CAMTIL="&CVG(CAMTIL)&" CAMRANG="&CVG(CAMRANG)
07000 CRLF);
07100
07200 END "CAM_UPDATE";
07300
07400 SIMPLE PROCEDURE HAT;
07500 BEGIN EXTERNAL PROCEDURE CALLEN;
07600 EXTERNAL INTEGER LENS;
07700 CALLEN; CAMLENS←LENS+1; CAM_UPDATE;
07800 END "HAT";
07900
08000 SIMPLE REAL PROCEDURE COSQR(REAL A,B,C);
08100 ⊃ Solves the eq. Acos(x)+Bsin(x)+C=0 for x;
08200 BEGIN FORTRAN REAL PROCEDURE ACOS(REAL X);
08300 FORTRAN REAL PROCEDURE SQRT(REAL X);
08400 REAL K,M,N;
08500
08600 K←A*C; M←A↑2+B↑2; N←B↑2-C↑2;
08700 IF A*B<0
08800 THEN RETURN(ACOS(-(K/M)+SQRT((K/M)↑2+(N/M))))
08900 ELSE RETURN(ACOS(-(K/M)-SQRT((K/M)↑2+(N/M))));
09000 END "COSQR";
00100 SIMPLE MESSAGE PROCEDURE CHNG_LENS(INTEGER RLENS);
00200 BEGIN EXTERNAL PROCEDURE CALLEN;
00300 EXTERNAL INTEGER STATUS,LENS;
00400 IF (RLENS<1)∨(RLENS>4) THEN BEGIN
00500 OUTSTR("CHNG_LENS-FAILED: LENS NO. OUT OF BOUNDS ("&CVS(RLENS)&")"CRLF);
00600 CAMFLG←2; RETURN; END;
00700 CALLEN;
00800 IF LENS≠RLENS-1
00900 THEN BEGIN LENS←RLENS-1; STATUS←'20; SERVO; CALLEN;
01000 IF LENS≠RLENS-1
01100 THEN BEGIN OUTSTR("CHNGE_LENS: I GOOFED, NOW YOU HAVE LENS NO. "&
01200 CVS(LENS+1)CRLF); CAMFLG←6; END; END;
01300 CAMLENS←LENS+1; CAM_UPDATE;
01400 END "CHNG_LENS";
01500
01600 SIMPLE MESSAGE PROCEDURE MOVE_CAM(REAL PAN,TILT);
01700 BEGIN REAL LL2,LL3;
01800 external real L2,L3, REF;
01900 EXTERNAL INTEGER STATUS;
02000 LL3←(PAN-PPOT0[CAMLENS])/PPOTD[CAMLENS];
02100 LL2←(TILT-TPOT0[CAMLENS])/TPOTD[CAMLENS];
02200 IF (LL2≥-340)∨(LL2≤-1870) THEN BEGIN
02300 OUTSTR("MOVE_CAM-FAILED: TILT OUT OF BOUNDS (LL2="&CVS(LL2)&")"CRLF);
02400 CAMFLG←4; END;
02500 IF (LL3≥2000)∨(LL3≤-2000) THEN BEGIN
02600 OUTSTR("MOVE_CAM-FAILED: PAN OUT OF BOUNDS (LL3="&CVS(LL3)&")"CRLF);
02700 CAMFLG←3; END;
02800 IF CAMFLG≠0 THEN RETURN;
02900 L3←LL3/REF; L2←LL2/REF; STATUS←'10; SERVO;
03000 CAM_UPDATE;
03100 END "MOVE_CAM";
03200
03300 SIMPLE MESSAGE PROCEDURE CHNG_FOCUS(REAL RANG);
03400 BEGIN REAL LL1;
03500 external real L1, REF;
03600 EXTERNAL INTEGER STATUS;
03700 LL1←(LNS(RANG,CAMLENS)-FOCLEN0[CAMLENS])/FOCLENG[CAMLENS];
03800 IF (LL1≥1900)∨(LL1≤0)
03900 THEN BEGIN OUTSTR("CHNGE_FOC-FAILED: FOCUS OUT OF BOUNDS (LL1="&
04000 CVS(LL1)&")"CRLF); CAMFLG←5; RETURN; END;
04100 L1←LL1/REF; STATUS←'10; SERVO;
04200 CAM_UPDATE;
04300 END "CHNG_FOCUS";
04400
00100 SIMPLE MESSAGE PROCEDURE CAM_CENTER(INTEGER RLENS; REAL XTC,YTC,ZTC);
00200 BEGIN REAL XCC,YCC,ZCC,D,RANG,PAN,TILT,LL1,LL2,LL3;
00300 external real L1,L2,L3, REF;
00400 EXTERNAL PROCEDURE CALLEN;
00500 EXTERNAL INTEGER LENS,STATUS;
00600 FORTRAN REAL PROCEDURE SIN(REAL X);
00700 FORTRAN REAL PROCEDURE COS(REAL X);
00800
00900 IF (RLENS<1)∨(RLENS>4) THEN BEGIN
01000 OUTSTR("CAM_CENTER-FAILED: LENS NO. OUT OF BOUNDS ("&CVS(RLENS)&")"CRLF);
01100 CAMFLG←2; RETURN; END;
01200
01300 XCC←P0[RLENS,1]-XTC; YCC←P0[RLENS,2]-YTC; ZCC←P0[RLENS,3]-ZTC;
01400 PAN←COSQR(YCC,-XCC,DP[RLENS,1]);
01500 D←XCC*COS(PAN)+YCC*SIN(PAN);
01600 TILT←COSQR(ZCC,-D,-DP[RLENS,2]);
01700 RANG←D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];
01800
01900 LL3←(PAN-PPOT0[RLENS])/PPOTD[RLENS];
02000 LL2←(TILT-TPOT0[RLENS])/TPOTD[RLENS];
02100 LL1←(LNS(RANG,RLENS)-FOCLEN0[RLENS])/FOCLENG[RLENS];
02200 IF (LL2≥-340)∨(LL2≤-1870) THEN BEGIN
02300 OUTSTR("CAM_CENTER¬FAILED: TILT OUT OF BOUNDS (LL2="&CVS(LL2)&")"CRLF);
02400 CAMFLG←4; END;
02500 IF (LL3≥2000)∨(LL3≤-2000) THEN BEGIN
02600 OUTSTR("CAM_CENTER-FAILED: PAN OUT OF BOUNDS (LL3="&CVS(LL3)&")"CRLF);
02700 CAMFLG←3; END;
02800 IF (LL1≥1900)∨(LL1≤0) THEN BEGIN
02900 OUTSTR("CAM_CENTER-FAILED: FOCUS OUT OF BOUNDS (LL1="&CVS(LL1)&")"CRLF);
03000 CAMFLG←5; END;
03100 IF CAMFLG≠0 THEN RETURN;
03200
03300 L1←LL1/REF; L2←LL2/REF; L3←LL3/REF;
03400 CALLEN;
03500 IF LENS≠RLENS-1 THEN BEGIN LENS←RLENS-1; STATUS←'30; END ELSE STATUS←'10;
03600 SERVO;
03700 CALLEN;
03800 IF LENS≠RLENS-1
03900 THEN BEGIN OUTSTR("CAM_CENTER: I GOOFED, NOW YOU HAVE LENS NO. "&
04000 CVS(LENS+1)CRLF); CAMFLG←6; END;
04100 CAMLENS←LENS+1; CAM_UPDATE;
04200
04300 END "CAM_CENTER";
00100
00200 SIMPLE MESSAGE PROCEDURE CAM_PRED(INTEGER RLENS; REAL XTC,YTC,ZTC);
00300 BEGIN REAL XCC,YCC,ZCC,D,RANG,PAN,TILT,LL1,LL2,LL3;
00400 FORTRAN REAL PROCEDURE SIN(REAL X);
00500 FORTRAN REAL PROCEDURE COS(REAL X);
00600
00700 IF (RLENS<1)∨(RLENS>4) THEN BEGIN
00800 OUTSTR("CAM_PRED-FAILED: LENS NO. OUT OF BOUNDS ("&CVS(RLENS)&")"CRLF);
00900 CAMFLG←2; RETURN; END;
01000
01100 XCC←P0[RLENS,1]-XTC; YCC←P0[RLENS,2]-YTC; ZCC←P0[RLENS,3]-ZTC;
01200 PAN←COSQR(YCC,-XCC,DP[RLENS,1]);
01300 D←XCC*COS(PAN)+YCC*SIN(PAN);
01400 TILT←COSQR(ZCC,-D,-DP[RLENS,2]);
01500 RANG←D*COS(TILT)+ZCC*SIN(TILT)-DP[RLENS,3];
01600
01700 LL3←(PAN-PPOT0[RLENS])/PPOTD[RLENS];
01800 LL2←(TILT-TPOT0[RLENS])/TPOTD[RLENS];
01900 LL1←(LNS(RANG,RLENS)-FOCLEN0[RLENS])/FOCLENG[RLENS];
02000 IF (LL2≥-340)∨(LL2≤-1870) THEN BEGIN
02100 OUTSTR("CAM_PRED¬FAILED: TILT OUT OF BOUNDS (LL2="&CVS(LL2)&")"CRLF);
02200 CAMFLG←4; END;
02300 IF (LL3≥2000)∨(LL3≤-2000) THEN BEGIN
02400 OUTSTR("CAM_PRED-FAILED: PAN OUT OF BOUNDS (LL3="&CVS(LL3)&")"CRLF);
02500 CAMFLG←3; END;
02600 IF (LL1≥1900)∨(LL1≤0) THEN BEGIN
02700 OUTSTR("CAM_PRED-FAILED: FOCUS OUT OF BOUNDS (LL1="&CVS(LL1)&")"CRLF);
02800 CAMFLG←5; END;
02900 IF CAMFLG≠0 THEN RETURN;
03000
03100 PANTIL_CAM (RLENS,LL3,LL2,LL1,MCOL,MICOL,LCEN);
03200 ARRBLT (CAMERA_PREDI[1,1],MCOL[1,1],9);
03300 ARRBLT (CAMERA_PREDI[6,1],MICOL[1,1],9);
03400 ARRBLT (CAMERA_PREDI[4,1],LCEN[1],3);
03500 CAMERA_PREDI[5,1] ← PP[CAMLENS,1];
03600 CAMERA_PREDI[5,2] ← PP[CAMLENS,2];
03700 CAMERA_PREDI[5,3] ← 1.0;
03800 CAMERA_PREDI[9,1]←PAN;
03900 CAMERA_PREDI[9,2]←TILT;
04000 CAMERA_PREDI[9,3]←RANG;
04100 CAMERA_PREDI[10,2]←RLENS;
04200
04300 END "CAM_PRED";
04400
04500 SIMPLE MESSAGE PROCEDURE CAM_INIT;
04600 BEGIN CAMFLG←0;
04700 FOR BLOCK←1 STEP 1 UNTIL 4 DO DATXFR; ⊃ JUST READ IN A MODEL;
04800 END"CAM_INIT";
00100 SIMPLE PROCEDURE TESTIT;
00200 BEGIN STRING COORD;
00300 REAL X,Y,Z,NPAN,NTILT,NRANG;
00400 INTEGER NLENS,REQUEST;
00500 LABEL TIT,TES;
00600 TIT: OUTSTR("1-CHANGE THE LENS"CRLF&
00700 "2-PAN AND TILT THE CAMERA"CRLF&
00800 "3-FOCUS ON A GIVEN RANGE"CRLF&
00900 "4-CENTER THE CAMERA ON A GIVEN POINT"CRLF&
01000 "5-COMPLEMENT DEB_CAM"CRLF);
01100 TES: OUTSTR("...TYPE THE NO. OF THE PROCEDUE YOU WANT TO TEST NOW="CRLF);
01200 REQUEST←CVD(INCHWL);
01300 IF (REQUEST<1)∨(REQUEST>5) THEN BEGIN
01400 OUTSTR("TESTIT-FAILED: ILLEGAL PROCEDURE NO ("&CVS(REQUEST)&")"CRLF);
01500 GOTO TIT; END;
01600 CASE REQUEST OF BEGIN
01700 BEGIN ⊃ 0; END;
01800 BEGIN ⊃ 1; OUTSTR("...TYPE NO. OF LENS="CRLF);
01900 NLENS←CVD(INCHWL); CHNG_LENS(NLENS); END;
02000 BEGIN ⊃ 2; OUTSTR("...TYPE PAN AND TILT IN RADS,"&
02100 " EACH FOLLOWED BY C.R="CRLF);
02200 COORD←INCHWL; NPAN←REALSCAN(COORD,MESS);
02300 COORD←INCHWL; NTILT←REALSCAN(COORD,MESS);
02400 MOVE_CAM(NPAN,NTILT); END;
02500 BEGIN ⊃ 3; OUTSTR("...TYPE RANGE TO FOCUS ON="CRLF);
02600 COORD←INCHWL; NRANG←REALSCAN(COORD,MESS);
02700 CHNG_FOCUS(NRANG); END;
02800 BEGIN ⊃ 4; OUTSTR("TYPE IN LENS N0."&
02900 "AND X,Y,Z OF CENTER IN TABLE COORDINATES"CRLF&
03000 "EACH FOLLOWED BY A C.R.="CRLF);
03100 COORD←INCHWL; NLENS←CVD(COORD);
03200 COORD←INCHWL; X←REALSCAN(COORD,MESS);
03300 COORD←INCHWL; Y←REALSCAN(COORD,MESS);
03400 COORD←INCHWL; Z←REALSCAN(COORD,MESS);
03500 CAM_CENTER(NLENS,X,Y,Z); END;
03600 BEGIN ⊃ 5; DEB_CAM←¬DEB_CAM; GOTO TES; END;
03700 END;
03800
03900 OUTSTR("...TYPE Y TO TEST AGAIN:"CRLF);
04000 IF INCHWL="Y" THEN BEGIN CAMFLG←0; GOTO TIT; END;
04100 END "TESTIT";
00100
00200 ⊃ HERE BEGINS EXECUTION ;
00300 CAMFLG←0; TYP_CAM←TRUE; DEB_CAM←FALSE;
00400 CAM_INIT;
00500 HAT;
00600 IF RUN=0
00700 THEN BEGIN OUTSTR("...TYPE Y FOR TEST MODE:"CRLF);
00800 IF YES THEN TESTIT; END
00900 ELSE BEGIN
01000 PUT_DATA(0,0,"CAM"); ⊃ DECLARE YOUR NAME ;
01100 OUTSTR("CAM-ACTIVATED"CRLF);
01200 YES_CAM←TRUE;
01300 WHILE TRUE DO BEGIN
01400 MESS ← GET_ENTRY ('120,NULL,"CAM",NULL);
01500 CAMFLG←0;
01600 MESS ← QUEUE ('600,MESS); ⊃ ACTIVATE AND ACKNOWLEDGE ;
01700 END; END;
01800 END "GILCAM";